import math
import numpy as np
import fme_base_functions as basefunction
import matplotlib.pyplot as plt


#
# def calculate_3t(ptO, ptA, ptB, ptC, radius, num_segments_on_arc, num_segments_per_section):
#     vOA = ptA - ptO
#     vOB = ptB - ptO
#     vOC = ptC - ptO
#
#     # 计算夹角
#     uOA = vOA / np.linalg.norm(vOA)
#     uOB = vOB / np.linalg.norm(vOB)
#     uOC = vOC / np.linalg.norm(vOC)
#
#     deltaAB = np.arccos(np.dot(uOA, uOB))
#     deltaAC = np.arccos(np.dot(uOA, uOC))
#     deltaBC = np.arccos(np.dot(uOB, uOC))
#     maxIndex = np.argmax([deltaAB, deltaAC, deltaBC])
#     pt_start, pt_end, pt_outside, pt_center, delta = None, None, None, ptO, None
#
#     if maxIndex == 0:
#         pt_start = ptA
#         pt_end = ptB
#         pt_outside = ptC
#         delta = 0.5 * deltaAB
#     elif maxIndex == 1:
#         pt_start = ptA
#         pt_end = ptC
#         pt_outside = ptB
#         delta = 0.5 * deltaAC
#     else:
#         pt_start = ptB
#         pt_end = ptC
#         pt_outside = ptA
#         delta = 0.5 * deltaBC
#     # 找出主方向
#
#     direction_cs = pt_start - pt_center
#     direction_ce = pt_end - pt_center
#     direction_co = pt_outside - pt_center
#     vertices_start = []
#     vertices_end = []
#     vertices_arc_segments = []
#     vertices_outside = []
#     vertices_center = []
#     vertices_se = []
#
#     distance = 0.5 * radius * math.atan(delta)
#     pt_start2 = basefunction.calculate_point_on_vector(-direction_cs, pt_center, distance)
#     pt_end2 = basefunction.calculate_point_on_vector(-direction_ce, pt_center, distance)
#
#     normal_sce = np.cross(direction_cs, direction_ce)
#     normal_ocs = np.cross(direction_co, direction_cs)
#     direction_start_first = np.cross(normal_sce, direction_cs)
#     direction_end_first = np.cross(direction_ce, normal_sce)
#     direction_outside_first = np.cross(normal_ocs, direction_co)
#
#     pt_start_first = basefunction.calculate_point_on_vector(direction_start_first, pt_start, radius)
#     pt_start2_first = basefunction.calculate_point_on_vector(direction_start_first, pt_start2, radius)
#     pt_end_first = basefunction.calculate_point_on_vector(direction_end_first, pt_end, radius)
#     pt_end2_first = basefunction.calculate_point_on_vector(direction_end_first, pt_end2, radius)
#     pt_outside_first = basefunction.calculate_point_on_vector(direction_outside_first, pt_outside, radius)
#     pt_center_first = basefunction.calculate_point_on_vector(direction_outside_first, pt_center, radius)
#
#     vertices_arc_segments = basefunction.calculate_pipe_params(pt_start, pt_center, pt_center, pt_end, distance, num_segments_on_arc,
#                                          num_segments_per_section,1, 2 * radius, 2 * radius, 2 * radius, 2 * radius)
#
#     for i in range(num_segments_per_section):
#         rotate_angle = (2 * i * np.pi) / num_segments_per_section
#         vector_tmp_start = basefunction.rotate_vector_around_axis(pt_start_first, direction_cs, rotate_angle)
#         u_tmp_start = vector_tmp_start / np.linalg.norm(vector_tmp_start)
#         pt_tmp_start = pt_start + u_tmp_start * radius
#
#         # vector_tmp_start2 = basefunction.rotate_vector_around_axis(pt_start2_first, direction_cs, rotate_angle)
#         # u_tmp_start2 = vector_tmp_start2 / np.linalg.norm(vector_tmp_start2)
#         # pt_tmp_start2 = pt_start2 + u_tmp_start2 * radius
#
#         vector_tmp_end = basefunction.rotate_vector_around_axis(pt_end_first, direction_ce, -rotate_angle)
#         u_tmp_end = vector_tmp_end / np.linalg.norm(vector_tmp_end)
#         pt_tmp_end = pt_end + u_tmp_end * radius
#
#         # vector_tmp_end2 = basefunction.rotate_vector_around_axis(pt_end2_first, direction_ce, -rotate_angle)
#         # u_tmp_end2 = vector_tmp_end2 / np.linalg.norm(vector_tmp_end2)
#         # pt_tmp_end2 = pt_end2 + u_tmp_end2 * radius
#
#         vector_tmp_outside = basefunction.rotate_vector_around_axis(pt_outside_first, direction_co, rotate_angle)
#         u_tmp_outside = vector_tmp_outside / np.linalg.norm(vector_tmp_outside)
#         pt_tmp_outside = pt_outside + u_tmp_outside * radius
#
#         vector_tmp_center = basefunction.rotate_vector_around_axis(pt_center_first, direction_co, rotate_angle)
#         u_tmp_center = vector_tmp_center / np.linalg.norm(vector_tmp_center)
#         pt_tmp_center = pt_center + u_tmp_center * radius
#
#         # pt_tmp_intersect = basefunction.find_intersection(pt_tmp_start, -direction_cs, pt_tmp_end, -direction_ce)
#         vertices_start.append(pt_tmp_start)
#         vertices_end.append(pt_tmp_end)
#         vertices_outside.append(pt_tmp_outside)
#         vertices_center.append(pt_tmp_center)
#
#         # vertices_se.append(pt_tmp_intersect)
#
#     # return deltaAB, deltaAC, deltaBC
#     # return ptStartA, ptStartB
#     return vertices_start, vertices_arc_segments,  vertices_end,  vertices_outside, vertices_center,

def calculate_nt(num_segments_per_circle, radius, pt_o, *pt_x):
    # v_OA = ptA - ptO
    # v_OB = ptB - ptO
    # v_OC = ptC - ptO
    # normal_AOB = np.cross(v_OA, v_OB)
    # normal_AOB = np.cross(v_OA, v_OB)
    # normal_AOB = np.cross(v_OA, v_OB)
    # print(pt_x[0])

    # num_segments_per_circle = 36
    pt_start = pt_o + np.asarray([0, 0, radius])
    normal_start = np.asarray([0, 1, 0])
    vertices_array = []
    vertices = []
    for i in range(num_segments_per_circle):
        rotate_angle1 = (2 * i * np.pi) / num_segments_per_circle
        rotate_normal = basefunction.rotate_vector_around_axis(normal_start, np.asarray([0, 0, 1]), rotate_angle1)
        for j in range(num_segments_per_circle):
            rotate_angle2 = (2 * j * np.pi) / num_segments_per_circle
            tmp_pt = basefunction.rotate_around_axis(pt_start, ptO, rotate_normal, rotate_angle2)
            vertices.append(tmp_pt)
    vertices_array.append(vertices)
    for i in range(len(pt_x)):
        tmp_start_vertices = []
        tmp_end_vertices = []
        pt_a = pt_x[i]
        v_oa = pt_a - pt_o
        tmp_v = np.cross(pt_a, v_oa)
        for j in range(num_segments_per_circle):
            rotate_angle = (2 * j * np.pi) / num_segments_per_circle
            rotate_v = basefunction.rotate_vector_around_axis(tmp_v, v_oa, rotate_angle)
            pt_start = basefunction.calculate_point_on_vector(rotate_v, pt_a, radius)
            pt_end = basefunction.calculate_point_on_vector(rotate_v, pt_o, radius)
            tmp_start_vertices.append(pt_start)
            tmp_end_vertices.append(pt_end)
        vertices_array.append(tmp_start_vertices)
        vertices_array.append(tmp_end_vertices)
    return vertices_array


if __name__ == "__main__":
    # num_segments_on_arc = 10
    # num_segments_per_section = 12
    # num_segments_per_circle = 36
    num_segments_per_circle = 24
    pipe_height = 0.2
    join_length = 0.3
    ptO = np.asarray([-0.9, 5.2, 0.6])
    ptA = np.asarray([-2.47, 1.87, 0.5])
    ptB = np.asarray([3.1, 6.5, 0.6])
    ptC = np.asarray([-1.1, 5.2, 5])

    # vertices = calculate_3t(ptO, ptA, ptB, ptC, 10)
    vertices = basefunction.calculate_nt(num_segments_per_circle, pipe_height * 0.5, join_length, ptO, ptA, ptB, ptC)
    n = len(vertices)
    print(n)
    # for i in range(1, len(vertices), 2):
    #     print(i)
    # print(len(vertices[2]))
    # num_segments_on_arc = 10
    # num_segments_per_section = 12
    # pipe_height = 1
    # vertices_start, vertices_start2, vertices_end, vertices_end2, vertices_outside, vertices_center, vertices_arc_segments = basefunction.calculate_3t(
    #     ptO, ptA,
    #     ptB, ptC,
    #     pipe_height,
    #     num_segments_on_arc,
    #     num_segments_per_section)
    # vertices_arc_segments = np.concatenate(vertices_arc_segments)
    # print(f'1. {a[0]},{a[1]},{a[2]}')
    # print(f'2. {b[0]},{b[1]},{b[2]}')
    # print(f'2. {b[0]},{b[1]},{b[2]}')
    # 可视化
    # fig = plt.figure()
    # ax = fig.add_subplot(111, projection='3d')
    # # ax.axis('equal')
    #
    # x = []
    # y = []
    # z = []
    #
    # for segment in vertices:
    #     for vertex in segment:
    #         x.append(vertex[0])
    #         y.append(vertex[1])
    #         z.append(vertex[2])
    #
    # # for v in [pt_start,pt_start2,pt_center,pt_end,pt_end2,pt_start_first]:
    # #     x.append(v[0])
    # #     y.append(v[1])
    # #     z.append(v[2])
    # ax.scatter(x, y, z, c='b', marker='o')
    #
    # # 设置坐标轴标签
    # ax.set_xlabel('X')
    # ax.set_ylabel('Y')
    # ax.set_zlabel('Z')
    #
    # # 显示图形
    # plt.show()
